#!/usr/bin/env python3
from __future__ import absolute_import
from __future__ import print_function
import rospy
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped

leader_odom_path = Path()
follower1_odom_path = Path()
follower2_odom_path = Path()

leader_odom_count = 0
follower1_odom_count = 0
follower2_odom_count = 0

leader_id = "/tb3_0/odom"
follower1_id = "/tb3_1/odom"
follower2_id = "/tb3_2/odom"

def leader_odom_cb(data):
    global leader_odom_path
    global leader_odom_count

    leader_odom_count = leader_odom_count + 1

    if leader_odom_count % 3 == 0:
        leader_odom_path.header = data.header
        leader_odom_path.header.frame_id = leader_id

        pose = PoseStamped()
        pose.header = data.header
        pose.pose = data.pose.pose
        pose.header.frame_id = leader_id
        leader_odom_path.poses.append(pose)

        leader_odom_path_pub.publish(leader_odom_path)

def follower1_odom_cb(data):
    global follower1_odom_path
    global follower1_odom_count

    follower1_odom_count = follower1_odom_count + 1

    if follower1_odom_count % 3 == 0:
        follower1_odom_path.header = data.header
        follower1_odom_path.header.frame_id = follower1_id

        pose = PoseStamped()
        pose.header = data.header
        pose.pose = data.pose.pose
        pose.header.frame_id = follower1_id
        follower1_odom_path.poses.append(pose)

        follower1_odom_path_pub.publish(follower1_odom_path)

def follower2_odom_cb(data):
    global follower2_odom_path
    global follower2_odom_count

    follower2_odom_count = follower2_odom_count + 1

    if follower2_odom_count % 3 == 0:
        follower2_odom_path.header = data.header
        follower2_odom_path.header.frame_id = follower2_id

        pose = PoseStamped()
        pose.header = data.header
        pose.pose = data.pose.pose
        pose.header.frame_id = follower2_id
        follower2_odom_path.poses.append(pose)

        follower2_odom_path_pub.publish(follower2_odom_path)
    

if __name__ == '__main__':
    rospy.init_node('record_all_trajectories_node')
    rospy.loginfo("record all trajectories!!")

    simulation = "yes"
    print(("simulation: %s" %simulation))
    simulation = rospy.get_param('~simulation')
    print(("simulation: %s" %simulation))
    leader_odom_topic = '/tb3_0/odom'
    follower1_odom_topic = '/tb3_1/odom'
    follower2_odom_topic = '/tb3_2/odom'
    if (simulation == "no"):
        follower1_odom_topic = '/tb3_1/fake_odom'
        follower2_odom_topic = '/tb3_2/fake_odom'
    
    print(("leader_odom_topic: %s" %leader_odom_topic))
    print(("follower1_odom_topic: %s" %follower1_odom_topic))
    print(("follower2_odom_topic: %s" %follower2_odom_topic))

    leader_odom_sub = rospy.Subscriber(leader_odom_topic, Odometry, leader_odom_cb)
    follower1_odom_sub = rospy.Subscriber(follower1_odom_topic, Odometry, follower1_odom_cb)
    follower2_odom_sub = rospy.Subscriber(follower2_odom_topic, Odometry, follower2_odom_cb)

    leader_odom_path_pub = rospy.Publisher('/tb3_0/recorded_path', Path, queue_size=10)
    follower1_odom_path_pub = rospy.Publisher('/tb3_1/recorded_path', Path, queue_size=10)
    follower2_odom_path_pub = rospy.Publisher('/tb3_2/recorded_path', Path, queue_size=10)
    rospy.spin()
